#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
#include <iostream>
#include <fstream>
#include <vector>

float mid_measure = 0;
bool save_flag = false;

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int nNum = scan->ranges.size();
    int nMid = nNum/2;
    mid_measure = scan->ranges[nMid];
    if(mid_measure == 0.0f)
        ROS_WARN("Measure[%d] = %f", nMid, mid_measure);
    else
        ROS_INFO("Measure[%d] = %f", nMid, mid_measure); 
    save_flag = true;
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"scan_to_file");
    
    ROS_INFO("scan_to_file start!");

    // 从参数获取文件名
    ros::NodeHandle n_param("~");
    std::string strFileName;
    n_param.param<std::string>("file_name", strFileName, "data");

    // 生成完整路径
    std::string strFile;
    char const* home = getenv("HOME");
    strFile = home;
    strFile += "/";
    strFile += strFileName;
    strFile += ".txt";
    ROS_WARN(strFile.c_str()); 

    // 清空这个文件
    std::ofstream file(strFile);
    file.close();

    ros::NodeHandle nh;
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);

    ROS_WARN("倒计时结束后将会开始采集");
    for(int i=5;i>=0;i--)
    {
        ROS_WARN("倒计时 - %d",i);
        ros::spinOnce();
        sleep(1);
    }


    int count = 0;
    ros::Rate r(2);
    while(ros::ok())
    {
        if(save_flag == true && count <= 100)
        {
            std::ofstream file(strFile, std::ios_base::app);
            if (file.is_open())
            {
                // 将前方测距数值按行写入文件
                file << mid_measure << std::endl;
                file.close();
                ROS_WARN("[%d]保存测距值 %f",count,mid_measure); 
                count ++;
            }
            else
            {
                std::cerr << "无法打开文件" << std::endl;
                break;
            }
        }
        ros::spinOnce();
        r.sleep();
    }

}
